#! /usr/bin/env python

from grid_map import GridMap
from bresenham import bresenham
import rospy
from utils import (convert_lidar_scan_to_local,
                   convert_odom,
                   robotic_2_inertial)
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry

P_prior = 0.5
P_occ = 0.9
P_free = 0.3

RESOLUTION = 0.03
MAP_NAME = "world"


# Main func
if __name__ == "__main__":
    x_lim = [-4, 4]
    y_lim = [-4, 4]
    rospy.init_node('mapping_node', anonymous = False)
    rate = rospy.Rate(10)

    # Create grid map
    gridmap = GridMap(x_lim, y_lim, RESOLUTION, P_prior)

    # mapping loop
    while not rospy.is_shutdown():
        scan_msg = rospy.wait_for_message('/scan', LaserScan)
        # TODO: acquire distance and bearing from scan
        distances, angles, information = convert_lidar_scan_to_local(scan_msg)

        odom_msg = rospy.wait_for_message('odom', Odometry)
        # TODO: acquire odom msg from odometry
        odom_x, odom_y, theta = convert_odom(odom_msg)

        # TODO: transform the coordinate from R to I
        x_measure_inertial, y_measure_inertial = robotic_2_inertial(odom_x, odom_y, theta, distances, angles)

        # TODO: mapping
        # Calculate variables for using Bresenham algorithm and Inverse_sensor_model
        x1 , y1 = gridmap.discretize(odom_x, odom_y)
        # Laser sensed points
        X2, Y2 = [], []
        # Simple forward model
        for (x_in, y_in, dist) in zip(x_measure_inertial, y_measure_inertial, distances):
            x2, y2 = gridmap.discretize(x_in, y_in)
            for (x_bres, y_bres) in bresenham(x1, y1, x2, y2):
                gridmap.update(x_bres, y_bres, p = P_free)
            if dist < scan_msg.range_max:
                gridmap.update(x2, y2, p = P_occ)
            X2.append(x2)
            Y2.append(y2)


